#include "Quaternion.h"
#include <cassert>

Quaternion::Quaternion(double _w, double _x, double _y, double _z){
	w=_w;
	x=_x;
	y=_y;
	z=_z;
}

Quaternion::Quaternion(double _w, Vec3d _v){
	w=_w;
	x=_v[0];
	y=_v[1];
	z=_v[2];
}

Quaternion::Quaternion(double _w, Vecd _v){
	assert(_v.Elts()==3);
	w=_w;
	x=_v[0];
	y=_v[1];
	z=_v[2];
}

Quaternion::Quaternion(Vec4d v){
	w = v[0];
	x = v[1];
	y = v[2];
	z = v[3];
}

Quaternion::Quaternion(Vecd v){
	assert(v.Elts()==4);
	w = v[0];
	x = v[1];
	y = v[2];
	z = v[3];
}

Quaternion::Quaternion(Vec3d axis, double angle){
	axisAngle(axis, angle);
}

double& Quaternion::operator[] (int i) {
	switch(i){
		case 0:	// w
			return w;
			break;
		case 1:	// x
			return x;
			break;
		case 2:	// y
			return y;
			break;
		case 3:	// z
			return z;
			break;
		default:	// error case
			printf("Error in quaternion indexing - index %d requested\n", i);
			exit(1);
			break;
	}
}

void Quaternion::operator =(Quaternion q){
	w = q.w;
	x = q.x;
	y = q.y;
	z = q.z;
}

Quaternion Quaternion::operator +(Quaternion q){
	Quaternion temp;
	
	temp.w = w+q.w;
	temp.x = x+q.x;
	temp.y = y+q.y;
	temp.z = z+q.z;

	return temp;
}

Quaternion Quaternion::operator -(){
	Quaternion temp;

	temp.w = -w;
	temp.x = -x;
	temp.y = -y;
	temp.z = -z;

	return temp;
}

Quaternion Quaternion::operator *(Quaternion q){
	Quaternion temp;
	temp.w = w*q.w - (x*q.x + y*q.y + z*q.z);
	temp.x = w*q.x + x*q.w + y*q.z - z*q.y;
	temp.y = w*q.y - x*q.z + y*q.w + z*q.x;
	temp.z = w*q.z + x*q.y - y*q.x + z*q.w;

	return temp;
}

Quaternion Quaternion::operator *(double n){
	Quaternion temp;
	
	temp.w = w*n;
	temp.x = x*n;
	temp.y = y*n;
	temp.z = z*n;

	return temp;
}

Quaternion Quaternion::operator /(double n){
	Quaternion temp;

	temp.w = w/n;
	temp.x = x/n;
	temp.y = y/n;
	temp.z = z/n;

	return temp;
}

double Quaternion::dot (Quaternion q){
	return w*q.w+x*q.x+y*q.y+z*q.z;
}
Quaternion Quaternion::conjugate(){
	Quaternion temp;
	
	temp.w = w;
	temp.x = -x;
	temp.y = -y;
	temp.z = -z;

	return temp;
}

double Quaternion::norm(){
	Quaternion conj = conjugate();
	Quaternion mult = conj*(*this);

	return (sqrt(mult.w));
}

void Quaternion::normalize(){
	double n = norm();
	if(fabs(n)>=EPSILON) *this = *this/n;
}

Quaternion Quaternion::inverse(){
	Quaternion temp = conjugate();
	double n = norm();
	if(fabs(n)<EPSILON) return temp;
	else return (temp/(n*n));
}

Vec3d Quaternion::getAxis(){
	Vec3d axis;
	Quaternion temp = *this;
	temp.normalize();
	double sin_half = sqrt(1-temp.w*temp.w);
	if(fabs(sin_half)<EPSILON) axis=vl_0;
	else {
		axis[0] = temp.x/sin_half;
		axis[1] = temp.y/sin_half;
		axis[2] = temp.z/sin_half;
	}
	return axis;
}

double Quaternion::getAngle(){
	Quaternion temp = *this;
	temp.normalize();
	return 2*acos(temp.w);
}

void Quaternion::axisAngle(double _nx, double _ny, double _nz, double _theta){
	double cos_half = cos(_theta/2);
	double sin_half = sin(_theta/2);

	/* normalize the axis if already not so */
	double n = sqrt(_nx*_nx+_ny*_ny+_nz*_nz);
	if(fabs(n)>=EPSILON) {
		_nx=_nx/n;
		_ny=_ny/n;
		_nz=_nz/n;
	}
	w = cos_half;
	x = sin_half*_nx;
	y = sin_half*_ny;
	z = sin_half*_nz;
}

void Quaternion::axisAngle(Vec3d _n, double _theta){
	axisAngle(_n[0], _n[1], _n[2], _theta);
}

Mat3d Quaternion::getMatrix(){
	Mat3d mat=vl_0;
	//dont normalize here !
	//should always be normalized before explicitly!!
	//normalize();

	mat[0][0] = w*w+x*x-y*y-z*z;
	mat[1][1] = w*w-x*x+y*y-z*z;
	mat[2][2] = w*w-x*x-y*y+z*z;

	mat[0][1] = 2*(x*y-w*z);
	mat[1][0] = 2*(x*y+w*z);
	
	mat[0][2] = 2*(x*z+w*y);
	mat[2][0] = 2*(x*z-w*y);

	mat[1][2] = 2*(y*z-w*x);
	mat[2][1] = 2*(y*z+w*x);

	return mat;
}

// get the derivative of rotation matrix wrt el no.
Mat3d Quaternion::getDerivativeMatrix(int el){
	Mat3d mat=vl_0;
	switch(el){
		case 0:	// wrt w
			mat[0][0]=w;
			mat[1][1]=w;
			mat[2][2]=w;
			mat[0][1]=-z;
			mat[1][0]=z;
			mat[0][2]=y;
			mat[2][0]=-y;
			mat[1][2]=-x;
			mat[2][1]=x;
			break;
		case 1:	// wrt x
			mat[0][0]=x;
			mat[1][1]=-x;
			mat[2][2]=-x;
			mat[0][1]=y;
			mat[1][0]=y;
			mat[0][2]=z;
			mat[2][0]=z;
			mat[1][2]=-w;
			mat[2][1]=w;
			break;
		case 2:	// wrt y
			mat[0][0]=-y;
			mat[1][1]=y;
			mat[2][2]=-y;
			mat[0][1]=x;
			mat[1][0]=x;
			mat[0][2]=w;
			mat[2][0]=-w;
			mat[1][2]=z;
			mat[2][1]=z;
			break;
		case 3:	// wrt z
			mat[0][0]=-z;
			mat[1][1]=-z;
			mat[2][2]=z;
			mat[0][1]=-w;
			mat[1][0]=w;
			mat[0][2]=x;
			mat[2][0]=x;
			mat[1][2]=y;
			mat[2][1]=y;
			break;
		default:
			break;
	}
	return 2*mat;
}

Mat3d Quaternion::getDerivativeMatrix(int el1, int el2){
	Mat3d mat=vl_0;

	// wrt same dof
	if(el1==el2){
		switch(el1){
			case 0:	// wrt w
                mat[0][0]=1;
				mat[1][1]=1;
				mat[2][2]=1;
				break;
			case 1:	// wrt x
                mat[0][0]=1;
				mat[1][1]=-1;
				mat[2][2]=-1;
				break;
			case 2:	// wrt y
                mat[0][0]=-1;
				mat[1][1]=1;
				mat[2][2]=-1;
				break;
			case 3:	// wrt z
                mat[0][0]=-1;
				mat[1][1]=-1;
				mat[2][2]=1;
				break;
		}
	}
	// wrt different dofs
	else {
		// arrange in increasing order
		if(el1>el2){
			int temp=el2;
			el2=el1;
			el1=temp;
		}
		switch(el1){
			case 0:	// wrt w
				switch(el2){
					case 1:	// wrt x
						mat[1][2]=-1;
						mat[2][1]=1;
						break;
					case 2:	// wrt y
						mat[0][2]=1;
						mat[2][0]=-1;
						break;
					case 3:	// wrt z
						mat[0][1]=-1;
						mat[1][0]=1;
						break;
				}
				break;
			case 1:	// wrt x
				switch(el2){
					case 2:	// wrt y
						mat[0][1]=1;
						mat[1][0]=1;
						break;
					case 3:	// wrt z
						mat[0][2]=1;
						mat[2][0]=1;
						break;
				}
				break;
			case 2:	// wrt y
				switch(el2){
					case 3:	// wrt z
						mat[1][2]=1;
						mat[2][1]=1;
						break;
				}
				break;
		}
	}
	
	return 2*mat;
}

void Quaternion::display(){
	printf("QUAT: %f, %f, %f, %f\n", w,x,y,z);
}

Vec3d RotatePoint(Quaternion q, Vec3d pt){
	Quaternion quat_pt(0, pt[0], pt[1], pt[2]);
	Quaternion qinv = q.inverse();

	Quaternion rot = q*quat_pt*qinv;

	// check below - assuming same format of point achieved
	Vec3d temp;
	//cout<<"Point before: "<<0<<" "<<pt.x<<" "<<pt.y<<" "<<pt.z<<"\n";
	//cout<<"Point after:  "<<rot.x<<" "<<rot.y<<" "<<rot.z<<" "<<rot.w<<"\n";
	temp[0]=rot.x;
	temp[1]=rot.y;
	temp[2]=rot.z;

	//cout<<"Point after rotation: "<<temp[0]<<" "<<temp[1]<<" "<<temp[2]<<endl;
	return temp;
}

Vec3d RotatePoint(Quaternion q, double x, double y, double z){
	Vec3d pt;
	pt[0]=x;
	pt[1]=y;
	pt[2]=z;

	return RotatePoint(q, pt);
}

// Returns an interpolated Quaternion ( = interpolated rotation)
Quaternion Slerp(Quaternion p, Quaternion q, double t){
	// Find shortest path on the sphere
	// If p and q too far apart, play a trick to get the shortest arc
	Quaternion result;
	//printf("\n dot product: %f\n", p.dot(q));
	if (p.dot(q)<0){
		q=-q;
		//printf("\n negation: %f, %f, %f, %f\n", q.x, q.y, q.z, q.w);
	}
	if ((p.dot(q))>EPSILON){	// angle sufficiently large
		// Slerp formula
		double w = acos((p.dot(q))/(p.norm()*q.norm()));	// The omega angle between the 2 Quaternions
		//printf("\n omega angle: %f\n", w);
		result = (p*sin((1-t)*w)+q*sin(t*w))/sin(w);
		//result.display();
	}
	else {	// so that sin(w) term doesnt blow - use Linear interpolation
		result = p*(1-t)+q*t;
	}

	//printf("displaying result\n");
	//result.display();
	return result;
}
